/*
 * CSE466 Au 2011
 *
 * Blimp Project
 *
 * (c) Dustin Richmon, Alex K. Horton, Jim Youngquist
 *
 * This is the main file for the blimp project.
 *
 */ 

#include <stdbool.h>

#include "msp.h"
#include "uart.h"
#include "motor.h"
#include "sensors.h"
#include "controller.h"

// To easily enable/disable timer 2 interrupts
#define TA2_ON  TA2CCTL0 |= CCIE
#define TA2_OFF  TA2CCTL0 &= ~CCIE

// At 32kHz / 8 ticks per second, 204 gives a 20Hz interrupt
//#define TA2_PERIOD 819 // 5Hz
//#define TA2_PERIOD 409 // 10Hz
#define TA2_PERIOD 204 // 20Hz

// Settings for the watchdog timer
// enable WDT, reset after WDT clock / 512k cycles, Clear counter, Use SMCLK
#define WDT_ON  WDTCTL = WDTPW | WDTIS_2 | WDTCNTCL | WDTSSEL__SMCLK //| WDTTMSEL
// disable WDT
#define WDT_OFF WDTCTL = WDTPW | WDTHOLD | WDTCNTCL

#define RED 1                   // on board LED
#define GREEN 0                 // on board LED

void initialize_cpu();                 // Sets up the DCO etc.
void set_led(uint8_t led, uint8_t on); // for red/green board leds

uint8_t sensor_buf[FRAME_DATA_SIZE];
//sensor_data_t *sensor_data = (sensor_data_t *)sensor_buf;
int16_t *sensor_data = (int16_t *)sensor_buf;

int main(void) {
    real_main();
    volatile int i;
    while(i++);
    return 0;
}

int real_main(void) {
    ////////////////////////////////////////
    // Local variables
    //uint8_t outbuf[] = " [ Zep is Go! ]\n";
    //outbuf[0] = 3;              // should me the 'MESSAGE' opcode
    uint8_t raw_command[FRAME_DATA_SIZE];
    uint16_t last_data_num = 0;
    uint16_t next_data_num = 0;
    uint16_t dropped_frames = 0; // when using non-blocking counts missed frames
    
    ////////////////////////////////////////
    // Stop the watchdog timer
    WDT_OFF;

    ////////////////////////////////////////
    // Initialize sub-systems
    initialize_cpu();
    initialize_UART();
    initialize_motors();
    initialize_sensors();
    initialize_controller();
        
    // Enable interrupts VERRA IMPOR'ANT
    __bis_SR_register(GIE);

    // Send welcome message
    //send_uart_data(outbuf, sizeof(outbuf)-1);
    set_led(GREEN, 1);

    ////////////////////////////////////////
    // Enter main loop
    //
    // The main loop is comprised of 4 main parts, each which is blocking in
    // its own right and separate from the rest.  Things should be timed so an
    // update cycle of at least 20Hz is possible.
    // 
    // NOTE - due to the use of the watchdog timer in the communication
    //        section, the PC must constantly send at least a heartbeat signal
    //        or the blimp assumes control has been lost and will go into a
    //        dormant phase of existance.
    while (1) {
        // 1 - Gather the sensor data
        gather_sensor_data(sensor_data);

        // pre-2 - kick the dog.  This way if comms take too long, we'll reset
        // and motors will turn off and everything will be in a nice, safe
        // state.  Also turn off TA2 interrupt in case it interferes with the
        // UART.
        TA2_OFF;
        WDT_ON;

        // 2 - Transmit the data
        uart_send(sensor_buf, FRAME_DATA_SIZE);
        
        //const unsigned char msg[10] = "Hello\n";
        //uart_send(msg, 6);

        // 3 - Wait for response
        //next_data_num = uart_receive_blocking(raw_command);
        next_data_num = uart_receive(raw_command);

        // post-3 - kill the dog.  UART operation seems to be the only place
        // that something goes wrong.  WDT is no longer needed.
        //WDT_OFF;
        
        // 4 - Apply opcodes
        if (next_data_num != last_data_num) {
            controller_tick(raw_command);
            dropped_frames = 0;
        } else {
            dropped_frames++;
        }

        // Indicate many dropped frames
        if (dropped_frames > 5) {
            set_motor_speed(0,0,0);
            set_led(RED, 1);
        } else {
            set_led(RED, 0);
        }
        
        last_data_num = next_data_num;

        // 5 - Sleep out the rest of this period if there is slack time
        if (TA2R < TA2_PERIOD) {
            TA2_ON;
            LPM0;
        } else {
            // Otherwise just reset the timer and hope we're fast enough next
            // time.
            TA2CTL |= TACLR;
        }
    }

    // We should never get here.
    return 0;
}

void initialize_cpu () { 
    UCSCTL3 = SELREF_2;         // Set DCO FLL reference = REFO
    UCSCTL4 |= SELA_2;          // Set ACLK = REFO
    UCSCTL0 = 0x0000;           // Set lowest possible DCOx, MODx

    // Loop until XT1,XT2 & DCO stabilizes - In this case only DCO has to stabilize
    do {
        // Clear XT2,XT1,DCO fault flags 
        UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + DCOFFG);

        // Clear fault flags
        SFRIFG1 &= ~OFIFG;      
    } while (SFRIFG1&OFIFG); // Test oscillator fault flag
	
    __bis_SR_register(SCG0);    // Disable the FLL control loop

    // (N + 1) * FLLRef = Fdcoclk
    // (511 + 1) * 32768 = 16.77MHz
    UCSCTL1 = DCORSEL_5;        // Select DCO range around 12MHz
    UCSCTL2 = 365;              // Set DCO Multiplier for 12MHz

    __bic_SR_register(SCG0);    // Enable the FLL control loop

    // Worst-case settling time for the DCO when the DCO range bits have been
    // changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
    // UG for optimization.
    // 32 x 32 x 8 MHz / 32,768 Hz = 250000 = MCLK cycles for DCO to settle
    volatile uint32_t i = 100000;
    while(i--);

    //SFRIE1 |= WDTIE;
    
    ////////////////////////////////////////
    // Initialize Timer A2
    // This is used to time the mainloop

    // ACLK, continuous mode, clear TAR, divide by 8.  This means that one
    // second is nominally 4096 ticks, or that TA2 can count up to 4 seconds.
    TA2CTL |= TASSEL_1 + MC_2 + TACLR + ID_3;
    TA2CCR0 = TA2_PERIOD;


    ////////////////////////////////////////
    // Set up the visible LEDs
    PJDIR |= BIT3;
    PJOUT &= ~BIT3;
    P5SEL &= ~BIT1;
    P5DIR |= BIT1;
    P5OUT &= ~BIT1;
}

void set_led(uint8_t led, uint8_t on) {
    if (led) {                  // red
        P5OUT = (P5OUT & ~BIT1) | (on<<1);
    } else {
        PJOUT = (PJOUT & ~BIT3) | (on<<3);
    }
}

// Timer1 A2 interrupt service routine
#ifdef __GNUC__
INTERRUPT(TIMER2_A0_VECTOR, Timer2_A0_ISR) {
#else
#pragma vector=TIMER2_A0_VECTOR
__interrupt void Timer2_A0_ISR(void) {
#endif
    // Simply reset the timer and start over
    TA2CTL |= TACLR;
    __bic_SR_register_on_exit(LPM0_bits);
}

#ifdef __GNUC__
INTERRUPT(WDT_VECTOR, wdt_isr) {
#else
#pragma vector=WDT_VECTOR
__interrupt void wdt_isr(void) {
#endif
    PMMCTL0 = PMMPW | PMMSWPOR; // software brownout reset
}
